﻿#ifndef CACULAER_H
#define CACULAER_H
#include <QDebug>
#include <QVector3D>
#include <QMap>
#include <QMatrix4x4>
#include <QVector>
#include "Eigen/Dense"
#include "Eigen/Core"


#include "opencv2/opencv.hpp"
#include "common.h"

static int cornerRow = 6;//行角点个数
static int cornerColum = 9;
static double patternLength=30.0;
static int imagewd= 640;
static int imageht= 480;


typedef struct Freme
{
    QVector<double> pos;
    QString imagepath;
    cv::Mat image;
    std::vector<cv::Point2f> center;
};
class Caculaer
{
public:
    Caculaer();
    ~Caculaer();
    enum class Pattern {CHESSBOARD, SYMCIRCLE, ASYMCIRCLE};
    class RobotPose{
    public:
        void updateTrans(double x,double y,double z);
        void updateQuat(double qw, double qx, double qy, double qz);
        void updateRotVec(double rx, double ry, double rz);
        void updateEulerAngle(double r, double p, double y);
        std::vector<double> getPose(std::string rot_rep = "rvec");
        std::vector<double> getPoseRPY(std::string rot_order = "xyz");
        Eigen::Isometry3f getHomoPose();

    private:
        double x{0},y{0},z{0};
        Eigen::Quaternionf quat = Eigen::Quaternionf::Identity();
        Eigen::AngleAxisf rot_vec = Eigen::AngleAxisf::Identity();
        Eigen::Matrix3f rot_mat = Eigen::Matrix3f::Identity();
        bool is_trans_set{false};
        bool is_rot_set{false};

    };
    void clear();
    void setdatasouce(QString path);
    bool execute(const QMap<int, Freme> &source);

    QMatrix4x4 getmatrixresult();
    QVector<double> geteulerreslut();
    QVector<double> getfourelementresult();
    bool loadImages();
    bool loadPoses();
    bool loadCamParams();


    //bool findPattern(Freme& frem, bool draw_result);//const cv::Mat& input,const cv::Size board_size,const Pattern& P,const std::string& img_name, std::vector<cv::Point2f>& center_output,bool draw_result=false);
    bool findPattern (const cv::Mat& input,const cv::Size& board_size,const Pattern& p,const std::string& img_name,std::vector<cv::Point2f>& centers_output,bool draw_result=false);
    bool findBoardPose(const cv::Size& board_size,const float& board_dim,const std::vector<cv::Point2f>& tracked_centers, const cv::Mat& camera_matrix, const cv::Mat& dist_coeffs,
        cv::Mat& rvec, cv::Mat& tvec);
private:
    QMap<int,Freme> fmap;
    std::vector<RobotPose> m_robot_poses;
    std::string imagefilepath;
    std::vector<cv::Mat> m_input_imgs;
    std::vector<std::string> m_img_names;

    std::vector<cv::Mat> m_target2base;
    cv::Mat m_R_camera2gripper; // rotation result
    cv::Mat m_t_camera2gripper; // translation result
    cv::Mat m_Homo_camera2gripper; // the homography matrix for camera to gripper transformation
    cv::Mat m_intrinsics; // the camera intrinsics
    cv::Mat m_distortion; // the distortion coefficients
    cv::Size m_expect_size;
    std::string m_result_dir{"./"};
    time_t m_rawtime;
    char* m_time_of_calib;
    bool m_calib_performed {false};


};

#endif // CACULAER_H
